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I. INTRODUCTION 



The detection, estimation and tracking of signals plays a significant role in many 
aspects of military and civilian operations. The Kalman filter has been used in tracking 
problems for many years. Its power comes from the mathematical foundation of statistical 
optimality. We investigate the behavior of the extended Kalman filter instead of using a 
linear Kalman filter, as most of the real world problems are non-linear. This thesis studies 
the detection of non-periodic signals, corrupted by zero mean, white, Gaussian noise using 
an extended Kalman filter algorithm. The parameters of the signals such as fundamental 
frequency, harmonic amplitudes and phases, are considered unknown and are estimated by 
the algorithm. 

In nature, most physical signals are approximately harmonic. This problem therefore, 
has applications in many fields including those of Radar and Sonar. An example of this is 
in the tracking of propeller driven platforms for submarines and helicopters. The sound of 
a rotary engine is almost periodic. If the engine speed changes, the sound of rotation 
changes, resulting in a change of frequency. The amplitudes and phases may also change 
slowly over time. Knowing the frequency, amplitude and the phase of the signal may allow 
identification of the class of vessel generating the sound. The frequency of the detected 
sound tells the speed of the vessel. The amplitude identifies the amount of vibration. 

Other applications include the estimation of harmonic signal parameters in the 
presence of noise to determine a radar’s modulated pulse repetition frequency or to 
investigate noisy biological signals such as heart wave forms. 
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This thesis is organized into six chapters. Chapters II and III explain the development 
of the Kalman filter and show the mathematical derivation of the extended Kalman filter 
algorithm. Chapters IV and V model the physical signal and show the simulation of each 
model. Chapter VI gives the conclusion of the report. The appendices contain program code 
applicable to this work. 
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II. KALMAN FILTER 



The object of this chapter is to show the development of the Kalman filter for 
estimating states from noisy data. The Kalman filter has the desirable quality of 
maintaining the physical meaning of the system dynamics by utilizing a state space 
representation. Here the state space model is used as the basic model. 

The optimality of the filter holds for systems which are linear and time-invariant 
(LTI), and corrupted by additive white Gaussian noise (AWGN). This chapter develops the 
Kalman filter equations for such a system. The development follows closely those given by 
Lewis, Gelb, and Candy [Ref. 2, 3, 4], 

A. DEFINITION OF TERMS 

All of the terms used throughout these chapters are defined in Table 2.1 and will be 
explained as they occur in the derivation. 

Terms with a single time subscript (i.e., x k ) refer to the value of the term at that time. 
Terms with dual subscripts (i.e., x k + ^ k ) refer to the value of the term at the time of the first 
subscript given observations through the second subscript. The third column of Table 2.1 
gives the dimension for each entry. 
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TABLE 2.1: DEFINITION OF TERMS 



System order: 


j 




Observation size: 


m 




System state: 


x k 


;xl 


Transpose of state 


T 

X k 


lx/ 


State transition matrix: 


0 


jxj 


State excitation noise: 


“>k 


jx 1 


Observation: 




m x 


Observation noise: 


v k 


m x 


State estimate: 


** + ll * 


jx 1 


Estimate error: 


x k+ 11 k 


jx 1 


Expected value of the error: 


£[**+11*1 


jx 1 


Error covariance matrix: 


^*A + 1| A + l 


jxj 


Residual: 


r A + l 


m x 



B. SYSTEM DYNAMICS 

A good model of the system is necessary for estimating parameters of that system. The 
state space representation of our linear system is given in Equation (2.1), and the measure- 
ment process is modeled as in Equation (2.2). 

** + i * **k + u >k (2-1) 

z k = Hx k + v k (2.2) 



4 



The physical state of the system (amplitude, frequency, etc.) is described by x and the 
observed parameters (amplitude, frequency, etc.) are described by observation z. Since the 
described system is time invariant, both and H are independent of time. 

The noise processes are considered as stationary, uncorrelated, zero-mean, additive 
white Gaussian noise (AWGN) processes. The statistical properties of the noise processes 
are given below 



E[w k ] = 0 


(2.3) 


Elu’jwl 1 = Q5 jk 


(2.4) 


w k ~ (0, Q) 


( 2.5) 


E[v k ] = 0 


(2.6) 


cr 

Per- ^ 

II 

So 

Sr 


(2.7) 


v k - (0, R) 


(2.8) 


E [WjvJ] = 0 


(2.9) 



where 5 is the kronecker delta function, defined by: 



8 = ( 1 ^7 ~ ^ 

J k 1 0 <=>y> k 



( 2 . 10 ) 



The matrices Q and R are non-zero, diagonal noise covariance matrices, which denote 
the power of the noise of the system. 

C. RECURSIVE SYSTEM 

Before presenting the detail of the Kalman filter equations, we will first introduce the 
linear and recursive forms of the filter, as shown in Equations (2.1 1) and (2.12). 
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( 2 . 11 ) 



( 2 . 12 ) 



K i is a system matrix, K 2 and G are the time- varying weighting matrices. The current 
estimate is a combination of the previous estimate and the current observation. 



The Kalman filter may be derived by optimizing the assumed form of the linear esti- 
mator. An optimal system is generally considered to be any system which either minimizes 
a cost function or optimizes a performance function. 

This experiment required the establishment of a filter that gives an unbiased estimate 
and minimizes error. An unbiased estimate has an expected error value of zero. The Con- 
stants K\ in Equation (2.11) and in Equation (2.12) are chosen to make the estimate un- 
biased. The constant G is chosen to minimize a cost function of the expected error. 

Following measurement, an equation for the estimation error can be obtained from 
Equation (2.12) by substitution of the measurement Equation (2.2) and the defining the re- 
lations as (tilde denotes estimation error): 



D. DISCRETE KALMAN FILTER 



**+11* - **+ 1 ~ x k + 1 | *+ 1 



(2.13) 




(2.14) 



The expected values satisfy 



E [** + 1| * + il - + - 0 



(2.15) 



and 
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*a + ii* + i - \K 2 + GH I]x k + j| k + K 2 x k + j| k + K 2 v h . (2.16) 

By taking the expected value of both sides and letting E [x* + J | A J = 0,E|i;J = 0,and 
E[** + il*+i] = 0 (i.e. unbiased estimator), the term in square brackets in Equation (2.16) 
is required to be zero. 



K 2 = I-GH (2.17) 

Combining Equation (2.14) and (2.17) gives the estimator the form of 

+ + i - [/- G//]x A + j, * + Gz k + l (2.18) 



or, alternatively 



**+ il * + 1 - ** + i*i +G[z k + , - Hx k+ xi fel * ( 2. 19) 

Following the same procedure, the value of K] can be determined by substituting 
Equations (2.1) and (2.11) into Equation (2.13) as below: 



*a + ha = <&x k + w k -K * - 4>i*| * . ( 2.20) 

By taking the expected value of both sides and applying Equations (2.3) and (2.15), 
we get 



K x = <t> 



( 2 . 21 ) 
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so that Equation (2.1 1) can be written in the form 



+ = ***!*• 



( 2 . 22 ) 



Equation (2.22) is known as the time update portion of the algorithm and gives the 
prediction x A + 1 | A of the state at time k+ 1, along with the associated error covariance 
P k + l | A . Also, Equation (2.19) is known as measurement update portion which provides a 
correction based on the measurement z k+l at time k + 1 to yield the net a posteriori estimate 
of x k + j and its error covariance of P k+1 \ k+l - This is known as predictor-corrector formu- 
lation of the discrete Kalman filter. 



To obtain a measure of confidence for the estimate, we need to find the error covari- 
ance. The error covariance matrices are defined by Equations (2.23) and (2.24). 



E. ERROR COVARIANCE UPDATE 



p, 



k + ii * 




( 2.23) 




( 2.24) 



The corresponding estimation error is of the form 



+ 1 | k + 1 1 GH ] x k + 1 | k k * 



( 2.25) 



Rewriting Equation (2.24) as 
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f*A + i| a + i ~ E { (I - GH)x k + 1| * (i* + 1| * (/- GH) T + v\G t ) + Gv k [x[ + ^ k (7 - GH) T + v^G T ] } 



( 2.26) 

and using Equations (2.23), (2.7), and the result of uncorrelated measurement errors we 
have, 

E 1*a+ i| a u a! = E [ + i| a) = 0> (2.27) 

and 

^a + ua + i = U-GH)P k + llk U-GH) T +GRG T . (2.28) 

The error covariance matrix gives the expected magnitude of the estimation error. 

F. OPTIMUM CHOICE OF KALMAN GAIN 

The criterion for choosing the gain is to minimize a weighted scalar sum of the diag- 
onal elements of the error covariance matrix. We will choose the value of G which satisfies 
Equation (2.29). 

G:min' G {J* + 1 } = £[x* + i| * + i** + i| * + il (2.29) 

Equation (2.29) can be written in terms of the error covariance as 

G:min' G {J* + 1 } = trace (P k + llk + 1 ) . (2.30) 

This is equivalent to minimizing the length of the estimation error vector. To find the 
value of the gain which provides a minimum, it is necessary to take the partial derivative 
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of J k + 1 with respect to G and set it to zero. Taking the partial derivative of J k + 1 requires 
the use of the matrices A and B, where B is symmetric. 



■^-tr ace (ABA 7 ) = 2 AB 
6A 



(2.31) 



Inserting Equation (2.28) into Equation (2.30) and applying Equation (2.31) gives the value 



-2(I-GH)P k + 1]k H T + 2GR = 0. (2.32) 

Solving for G, we have 



G = P k + 1 \ k H T [HP k + l{k H T + R)- 1 (2.33) 

which is referred to as the Kalman gain matrix. 

G. KALMAN FILTER EQUATIONS 

The set of recursive equations in Table 2.2 provide the time varying optimal gain ma- 
trix and the error analysis of the estimate. 
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TABLE 2.2: KALMAN FILTER EQUATIONS 



Time update (effect of system dynamics) 



Error covariance: 



*>* + n* = <i>P klk <t> T + Q 

Estimate: 


( 2.34) 


** + H* = ^**1* 

Measurement update (effect of measurement z ) 
Error covariance: 


( 2.35) 


P k + ,|*= 1 (^* + 1| *) _1 + H T R~ l H ] _1 

Residual: 


( 2.36) 


^ k + 1 — + 1 — + 1| h 

Estimate: 


( 2.37) 


*k + 1 | k + 1 = ** + 1 | * + Pk + 1 | *+ \H t R ' U* + j - Hx k+ ( ) 
Alternative Measurement Update Equations: 


( 2.38) 


G = P k + Uk H T (HP k + ]]k H T + R)-' 


( 2.39) 


Pk+ 1| * = - GH) P k + ]| k 


( 2.40) 


+ 1| k + 1 — * k + 1| k + ^ + i| 


(2.41) 



These equations require the following initial conditions, i oj 0 and P 0 | 0 . 
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There are many equivalent formulations for Equations (2-33) and (2-35). Using the 
matrix inversion lemma the latter can be written as 

+ + i = Pk + i l k-Pk + i\kH T lHP k . llt H T +R]- 1 HP k . lik , (2.42) 

Note, Equation (2.42) requires only a pxp matrix inversion. Equation (2.36) requires the 
inversion of two vxv matrices, and the number of measurements for p is usually less than 
for v. If |P* + j| *| = 0, then Equation (2.44) must be used. 

If it is often convenient to replace the measurement update equations by alternative 
equations such as those in Table 2.1. 

The Kalman filter gain is the weighting that determines the influence of the residual 
in updating the estimate. It is important to remember that the equations of Table 2.2 are only 
optimal when the system is linear, and when the a priori knowledge of the noise is avail- 
able. 
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III. EXTENDED KALMAN FILTER 



In this section, an approximate solution to the non-linear filtering problem defined 
below is developed. This solution involves the linearization of a non-linear process 
traversing about a reference trajectory and the modification or extension of the linear 
Kalman filter algorithm using the linearized model. 

In practice, many processes are non-linear rather than linear. Coupling non-linearities 
with noisy data makes the signal-processing problem a challenging one. Instead of 
extending this solution to the continuous case, the extended Kalman filter algorithm was 
developed to test for the discrete non-linear system. [Ref. 2] and [Ref. 3]. 

A. SYSTEM DYNAMICS 

Let us discuss the non-linear system of the form 

** + i = <&** + “'* (3.1) 

Z k + 1 = ^ (** + i) + V k + 1> (3.2) 

where w k ~ (0, Q) and v k ~ (0 ,R) are white noise processes uncorrelated with each other. 

B. APPROXIMATE MEASUREMENT UPDATE 

In order to find a measurement update that can be conveniently programmed, h ( x k + j) 
is expanded into a Taylor series about i* + lj *, an a priori estimate at time £+1. 
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*(*a + i) = M** + h*) 

dx 



(** + 1 ~*k+ i| *) + higher order terms ( 3.3) 



1| J 



Neglecting the higher order terms (H.O.T.) 



+ - h (** + i| k) + ^ 

dx 



x k+i\k 



(3.4) 



x - *A + 1| k 



representing the Jacobian as; 

H (x,k) = -$-h(x,k), (3.5) 

dx 

using Equations (3.4) and (3.5) can be used to proceed with the Kalman filter analysis. 

C. APPROXIMATE TIME UPDATE 

To obtain a complete filtering algorithm, update equations that account for measure- 
ment data are needed. We choose a linear, recursive set of equations for the estimate, 

*A+1|A+1 = a k + 1| k + + \ Z k + 1 » (3.6) 

where the vector a k and the gain matrix G k are to be determined. Proceeding with 
arguments similar to those used in Chapter II, we define the estimation error as 

*A + 1| k + 1 =** + 1| A + 1 ~ x k+ 1 (3.7) 

k + 1| k + 1 = + 1| k ~ x £ + 1 . (3.8) 
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Equations (3.6) and (3.7) and (3.8) are combined with Equation (3.2) to produce the 
following expression for the estimation error: 

*a + iia + i — + G k + (x k + 1 ) *** G k + 1 v k + 1 + 1| a ” + 1 | k (3.9) 

One required condition is that the estimate is unbiased. Applying this requirement to 
Equation (3.9) and letting E[x k + l \ k ] = E [i> A ] =0, we obtain 

a *+i + ^* + i^(^* + i|*) _ ^*+i|* = 9. (3.10) 

By defining the residual as the difference between the observation and the expected 
value of the observation in Equation 3.11 

<?* = i = Ak + i-^(** + i|*) > ( 3.11) 

solving Equation (3.10) for a A + I , and substituting the result into Equation (3.6), and 
combining it with Equation (3.1 1) yields the extended Kalman filter estimate equation 

Xk + 1| k + l = x k + i| * + G k + + 1 • (3.12) 

D. ESTIMATION ERROR COVARIANCE 

Using the definition of error covariance 

Pk + 1| * = E (** + 1| *** + 1| *1 (3.13) 
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and 



p * + ii * + 1 - & [**+ ii *+i^r+ii *+ii 



(3.14) 



the error-update equations may be derived. Taking Equation (3.10) into account, Equation 
(3.9) becomes 



* a + i|a + i = x k + x \ k + G k + 1 [h(x k + 1 ) -h(x k + ,)] + G k + 1 v k + 1 . (3.15) 

Using this, to generate the error covariance P k+1 in terms of the yet undetermined 
G k + j we can write 



^A + l| A + l - ^*A + 1| A + ^A + l^ ( 1^ (*A + l) 4 (X k + j) ] [h (*a+ j) 4(* i+1 )] }G* + 1 + 
E {X A + n A [h (x k + 1 ) - h (Xa + 1 ) ] T } Gl + ! + 

G k + \E { [ h (x k + 1 ) -h(x k + 1 )]xl + 1 | *} + G k + l R k + l Gl +1 , (3.16) 

where 



R k = E[v k v T k ] . 



The gain G A + 1 is now selected to minimize P k+l . Differentiating P k + 1 with respect 
to G a + 1 and solving for G k + i results in the desired optimal gain matrix. 
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G k + \ = £ { (** + i|*> 1M** + i) - ^(i* + i)) 7 } x 

{£{ [/i(x* + 1 ) -/i(i* +1 )] [*(x* +1 ) -h (x k + i)] T } + R k + l }~'. (3.17) 
Substituting this into Equation (3.16) and simplifying yields 

Pk + 1| * + 1 = p * + i|* + G * + i^{ t* (x k + ,) - h ( £* + i)l^* + i|*}- (3. 18) 

The complete linear estimate update due to non-linear measurement is given by Equa- 
tions (3.15), (3.18) and (3.19). However, these equations are impractical to implement be- 
cause they depend on conditional moments of x k+ , in computing h (x k + ,) . In order to sim- 
plify the computation, we will expand h(x k + i ^ k + i ) into a power series about x k + ,j * as fol- 
lows: 



^(**+n*+i) = M** + ua) +H(x k+l{k ) (** + n* + i-i* + i|*) +••• (3.19) 



where 



H(x k + 



*> = aT (x) 



x = i*. 



II * 



Truncating the above series after the first two terms, substituting the resulting approx- 
imation into Equations (3.18), (3. 19), and carrying out the indicated expectation operations 
results in a measurement error covariance update as follows: 



P 



* + ii * + 1 



l^ - G k + + 1| *) ) •f > *+ 1| *• 



( 3.20) 
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This equation represents an approximate, linear measurement update for the error co- 
variance. The residual is computed using the non-linear measurement function h (x) eval- 



uated about the a priori estimate x* + 1 | k . The error covariance is found using the Jacobian 
matrix. 

TABLE 3.1: EXTENDED KALMAN FILTER EQUATIONS 

System model and measurement model: 


X = f(x,t) +w(t) 


(3.21) 


z k = h(x ( k )) + v k 


( 3.22) 


w(t) - (0.Q), v k ~ (0,R) 

Time update: 

Estimate (state prediction): 


( 3.23) 


** + i| * = f*k\ k 

Jacobians: 


( 3.24) 


*(x.O = 4-fix, t) 

OX 


( 3.25) 


H(x) = (x,k) 

dx 

Error covariance (covariance prediction): 


( 3.26) 


^* + il* = + u * ( ^ >7 ' + Q 

Measurement update: 

Kalman gain: 


( 3.27) 


^ + 1 — J 5 * + 1| + 1| k) 1 H {ih + 1| h) Pfr + j| frH (Xk + 1| k) ^1 

Error covariance (covariance correction): 


( 3.28) 


+ + i = + + 1| *) ]Pk + 1| * 

Estimate (state correction): 


( 3.29) 


+ 1| k + 1 — + 1| A + 1 + 1 — ^ (x fa + i| a) 1 


( 3.30) 
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The covariance and gain equations are identical to those in Table 2.2, but with the Ja- 
cobian values A and // linearized about x k + ,, * . Note that P and G are now functions of the 
current state estimate, which is a conditional mean and therefore a single realization of a 
stochastic process. It also should be realized that the measurement times do not need to be 
equally spaced. The time update is performed over any interval during which no data are 
available. When data become available, a measurement update is performed. This means 
that an indication of missing measurements can be obtained, and pure prediction in the ab- 
sence of data can easily be achieved by using the extended Kalman filter. 

Higher-order approximations to the optimal non-linear updates can also be derived 
by retaining higher-order terms in the Taylor series expansions. 
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IV. POLAR MODEL OF THE SYSTEM 



Consider an approximately periodic, non-sinusoidal signal, in additive white 
Gaussian noise. A non-sinusoidal signal may be considered to consist of an infinite number 
of sinusoidal components. Three sets of parameters can characterize the signal: the 
fundamental frequency, the amplitude of each harmonic component, and the phase of each 
harmonic component. 

The signal is not exactly periodic since frequencies, amplitudes and phases change 
slowly over time. We will keep the same notation as defined in Nehorai and Porat [Ref. 6] 
and Parker and Anderson [Ref. 7]. For this purpose, we will first take a periodic signal y (t) 
with a zero d.c. component. A Fourier series representation of this signal can be written as: 



y(t) = ^ r k sin (kw ft + $ k ) (4.1) 

* = l 

In this paper a discrete time domain (i.e. t = 0,1,2,...) rather than a continuous domain 
will be used. As our signal y(t) is not exactly periodic, but has a slowly time varying 
frequency w f , amplitudes r k , and phases V we can state 



U) f = w f (t) 
r = r k (<) 

4>* = VO 
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(4.2) 

(4.3) 

(4.4) 



where w f {t) , r k (t) and 4 > A (<) are nearly constant over a few periods of y(t). In other 
words; 



\w f (t + 1) -w f (t)\ 

w f (t) 

M<+ O -M0| 

w f (t) 

IV' + D-VOI 

u> f (t) 



(4.5) 

(4.6) 

(4.7) 



In the model of y(t), the quantities w f (t) , r k (t) and <b k (t) are the instantaneous 
frequency, amplitudes, and phases of the signal. We will call the model given in Equation 
(4.1) the polar model and the model given in Chapter V the rectangular model [Ref.7]. 

We assume for both models that the signal y(t) is corrupted by noise. The 
measurements are given by 



2(0 = y(0 +v (0- ( 4.8) 

The task is to estimate the values r, (t) ,...,r m (0 ,w f (t ) , 4», (0 ,... <t> m (0 from the mea- 
surements, where m denotes the number of the significant harmonics. Parameters are only 
estimated up to mth harmonics. The higher harmonics are assumed to be negligible. A total 
of 2m+l parameters must be estimated. 

As explained in Parker and Anderson [Ref.7], we are also estimating amplitudes as 
well as the fundamental frequency. This is required to establish an estimator that uses both 
the energy in the fundamental and the energy in the higher harmonics to estimate the fre- 
quency of the signal. The information about the frequency contained in any harmonic de- 
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pends on the energy in that harmonic. If a particular harmonic component is strong, then 
the estimator of the frequency components should give more weight to the information 
available in the strong harmonic and less weight to the information available in the weak 
harmonics. 

Estimation of the harmonic amplitude also assists in estimating the frequency. The es- 
timator determines the frequency by first estimating the harmonic amplitudes. Knowledge 
of the frequency and the phases in the polar model also assists in the calculation of the har- 
monic amplitudes. 

A. THE ESTIMATOR 

Given a noisy measurement sequence and the associated models as shown in Fig. 
4. l[Ref.4], the Kalman filter can be thought of as an estimator that produces three types of 
outputs. The first filter can be named as a state estimator or reconstructor. It reconstructs 
estimates of the state x{t) from noisy measurements y(t) . This kind of model may be 
thought of as the means to implicitly extract x (t) from y ( t ) . 

The second filter, may be thought of as a measurement filter that accepts noisy se- 
quences of input and produces a filtered sequence of output. Finally, the estimator can be 
thought as a whitening filter that accepts noisy correlated measurements and produces un- 
correlated or white residues e (t) = z (t) - z (<| t - 1) . For our purposes, we will concentrate 
on the measurement filter form. 
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x (<| 0 



y(t) 



y(t) 



y(t) 




y(t\t) 



e(t) 



Figure 4.1 Various representations of the Kalman filter 

As mentioned before, the extended Kalman filter will be used to estimate frequency, 
phases, and amplitudes of an almost periodic signal imbedded in noise. First, we need a 
state space representation of the signal defined in Equation (4.1). Therefore, 

x(/+ 1) = Qx (t) + w (t) (4.9) 

z(0 = y(t) +v(t) ( 4.10) 

2(0 = h(x(t)) + v(t) (4.11) 



and 



x(t) =[r , (/) , r 2 (0 



,...,r m (0 , w f (t ) , (0 , <}> 2 (0 ,-,4> m (0 ] T 



(4.12) 



and 
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<s> = 



(4.13) 



/«0 0 
0 1 0 
.0 0 I m _ 



where, I m is a mth order identity matrix. 



h(x(t)) = ^r k (t) sin (kWft + $ k ) (4.14) 

k = l 



and w (t) is white Gaussian noise, with a zero mean and a variance 

E[w(t)w T (t)] = Q. (4.15) 

The observation noise v ( t ) is also white Gaussian noise, with zero mean and has a 
variance 



E [u (t) v T (t) ] = R 



and is uncorrelated with w{t) . 



(4.16) 

(4.17) 



E[w(t)v(t)] =0 (4.18) 

We will have a Q matrix which is diagonal. Its value will be given in the simulation 
section. From Equation (4.9), it can be concluded that the harmonic amplitudes evolve 
randomly over time. Also, the same argument is true for w f (t) , the fundamental frequency, 
and the ( t ) phases of the signal. The rate of the random walk will be determined by the 
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diagonal Q matrix. A zero Q matrix will correspond to constant amplitude, frequency and 
phase. In order to estimate x o| t) or x 0| t - 1) of x o ) from the measurement z (0 , the ex- 
tended Kalman filter will be applied. Here x(t\t) denotes the estimation of x(t) , given 

measurements {z(t)| t = 0, l, 2 t) up to and including time t. The value x 00 - 1) is an 

estimate of x ( t ) , given z( t) up to time r-1. 



*00) = *00- 1) + G0) 00) -h(x(t\t- 1))] (4.19) 

x(t + l|<) = <D*o0) (4.20) 

G (0 = P(t)H T (t) (H{t)P{t)H T {t) +R)~ ] (4.21) 

P(t + 1) = <h[P(0 - G (t) H (t) P (t) ] <t> T + Q (4.22) 



where H (t) is the Jacobian of h 0 ) . That is: 



dh (x 0| t - 1) ) 

1 ) 



sin (uyOl t - 1)< + tjq 01 t - 1) ) 
sin (u>f(t\t - 1) t + <t> 2 00 _ 1) ) 



-i 



H(t) 



sin (uy 0| t - 1) t + <j> m 00 - 1) ) 

3 0 |t - 1) 

r\ 00- 1) cos (uyOl t- 10 + (jy 0| t - 1) ) 



r 2 00 - 1) cos (uy 01 t - 1) t + <j> 2 00 ~ 1) ) 



r m 0| t - 1) cos (uyOl t - 1) t + <t> 3 00 - 1) ) 



( 4.23) 
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where, 



4 

3(/|/-l) = £^/cos(uy(/|/- l)/ + 4>*(/|/-l)) 
k = 1 



and the initial values are 



X (0) = £[x(0)] = x(0) 



and 



P( 0) = £[(x( 0)-x(0))(x(0)-x(0)) r ]. 



B. SIMULATIONS 

In the simulation we let 



z (t) = rjsin (27i0.04/ + 0.01) +r 2 sin (47i0.04< + 0.01) +r 3 sin (6n0.04< + 0.01) + 
r 4 (8710.04/ + 0.01) +u (0 (4.24) 

where v (/) is a zero mean, white Gaussian noise process with a variance of 0.1. The 
fundamental frequency of the signal w f = 27t0.04 and the amplitudes of the signal r k are 
constant over time. We use the amplitudes as written below. 



r 



k - 



' A- 1 



2 



* = 2,3,4, (4.25) 
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where r t is chosen for a desired signal to noise ratio as given by 



SNR = 10 




( 4.26) 



For the simulations, the filter program was written in MATLAB (See Appendix A), 
with the following initial conditions: 



rj (0) - 2.4 ( 4.27) 

r 2 (0) = 0 (4.28) 

r 3 (0) = 0 (4.29) 

r 4 (0) = 0 (4.30) 

0^(0) = 2*0.03 (4.31) 

4>! (0) = 0 (4.32) 

<M°) = 0 (4.33) 

$> 3 (0)=0 (4.34) 

4> 4 (0) = 0 (4.35) 

P(0) = diag {6,5,3,2,0.08,0.01,0.01,0.01,0.01} (4.36) 



As stated in Candy [Ref. 4], the variation of the gain is related to the variations in P 
and Q. Uncertainty in the model can be characterized by the process noise covariance, Q. 
For a large Q, P is large, which indicates a high uncertainty or an inadequate model. For 
small Q, P is small, indicating an adequate model. Therefore, the Kalman gain can be 
thought of as a ratio of process to measurement noise; that is. 
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( 4 . 37 ) 




It can be seen that the variations in G are proportional to the variations of Q. The Kal- 
man estimator can be perceived as a deterministic filter with a time-varying bandwidth as 
determined by the filter gain. As Q increases, both G and the filter bandwidth increase. 
Thus, the filter transient performance will be faster. The same effect can be observed by a 
small R. Conversely, if Q decreases, G decreases, which decreases the bandwidth of the fil- 
ter, and hence, the filter transient response is slower. 

The steady-state value of the error covariance matrix P increases (decreases) with a 
corresponding increasing (decreasing) value of the Q matrix. We conclude that P is also 
proportional to Q. We also note a similar effect by varying R. 

We investigated the effect of changing the initial conditions of P on the algorithm per- 
formance. As long as the filter is stable and the state-space model is completely controllable 
and observable, then 



Thus, an estimator error approaching zero implies that the state estimate converges to 
a true value, given that enough data exists. The initial estimates will affect the transient per- 
formance of the algorithm, since a large initial P( 0) gives a large G( 0), and therefore 
heavily weights the initial measurements and ignores the model. 

In the program, we used R = 0.1, while 



HmP(0 =P (small). 



( 4 . 38 ) 



Q — ditlg (Qj >Q i<Q2>Q2’Q2’^2’^2 1 



( 4 . 39 ) 
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where 



g, = 1 x 1CT 3 and q 2 = 1 x 10 7 . 

The entries of Q and R roughly determine the bandwidth of the estimator. This also 
affects the capture properties, tracking properties, and steady-state error. The capture prop- 
erty can also be influenced by the choice of P(0) . The values used in this work were largely 
obtained by trial and error. 

Figure 4.6 shows the true and estimated values of the fundamental frequency from 
450 samples. The estimated value of the frequency tracks the true value after t - 30 and 
locks onto it very closely. Figure 4.2 shows the true and the estimated values of the ampli- 
tude of the First harmonic component. Tracking is achieved after t - 50. Figure 4.3 shows 
the estimated and the true amplitudes of the second harmonic component. Tracking is 
achieved after t = 50, although the initial estimate value r 2 (0) is zero. The other harmonic 
amplitudes and their estimates show similar responses, as in Figures 4.4 and 4.5. 

The phase error of the fundamental component is shown in Figure 4.7. The error is 
close to zero after t - 350. A similar response can be seen in Figure 4.8 for the phase error 
of the second harmonic. Figure 4.9 shows the phase error of the third harmonic. At t = 450, 
the error is 0.00025. Figure 4. 10 shows the phase error of the fourth harmonic, which has a 
steady state error of 0.007 after t = 250. The phase error of the second and the higher har- 
monics is higher than in the first harmonic. This is probably because the initial estimate of 
the second and the higher harmonic amplitudes were zero. 

Since higher harmonics are assumed to be negligible we only considered tracking sig- 
nals with less than four harmonics. For one case, we accepted a signal consisting of two 
harmonics. For the simulations, we let 
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z(t) = r 1 sin (2n0.04t + 0.01) +r 3 sin (6n0.04£ + 0.01) +u (t) 



(4.42) 



and the second and the fourth harmonics are assumed to be missing. The filter could not 
initially track the initial error covariance matrix used with four harmonics. So we 
rearranged the initial value of the error covariance matrix as 

P( 0) = diag{ 2, 1,0.5,0.2,0.04,0.01,0.01,0.01,0.01} . (4.43) 

We used the MATLAB program in Appendix B for the simulations. The results are 
given in Figures 4.11 to Figure 4.15. Figures 4.11 and 4.12 show the true and estimated 
values of the first and the third harmonics, respectively. For both cases tracking is achieved 
after t = 30. The second and the fourth harmonic true and estimated values were around 
zero. These graphs are not included. Figure 4. 13 shows the frequency tracking of the signal. 
Its transient response was similar to the one obtained for the case of four harmonics. Figures 
4.14 and 4.15 show the phase error of the first and the third harmonics, respectively. The 
phase error of the third harmonic was worse than the first harmonic because of the poor ini- 
tial value of the third harmonic. 
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Figure 4. 2 True and the estimate value of the amplitude of the first harmonic 




Figure 4. 3 True and the estimate value of the amplitude of the second harmonic 
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Figure 4. 4 True and the estimate value of the amplitude of the third harmonic 




Figure 4. 5 True and the estimate value of the amplitude of the fourth harmonic 
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Time 



Figure 4. 6 True and the estimate value of the fundamental frequency 
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Figure 4. 7 Phaseerror of the first harmonic 




Figure 4. 8 Phase error of the second harmonic 
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Figure 4. 9 Phase error of the third harmonic 




Figure 4. 10 Phase error of the fourth harmonic 



35 




Figure 4. 11 Amplitude tracking of the first harmonic. (Two harmonic case) 




Figure 4. 12 Amplitude tracking of the third harmonic. (Two harmonic case) 
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Figure 4. 13 Frequency tracking. (Two harmonic case) 




37 




Figure 4. 14 Phase error of the first harmonic. (Two harmonic case) 



xlO 3 




Figure 4. 15 Phase error of the third harmonic. (Two harmonic case) 
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V. RECTANGULAR MODEL OF THE SYSTEM 



This chapter applies the extended Kalman filter to the signal formulated in Chapter IV, 
but represents it in a different way. The signal is not periodic but has frequency, amplitude 
and phases that change slowly over time in a similar way to that in Chapter IV. Three sets 
of parameters (frequency, amplitude, and phase) define the signal. We will call the 
parameterization in this chapter the rectangular model as defined by Anderson and Parker 
[Ref. 7] and Anderson and James [Ref. 8]. In this representation y (t) has amplitudes and 
phases with sine and cosine components, like 



y(t) = a, (f) sin (w^(t)t + 4> (0 ) + [a* (f) sin (k (w f (t) t + <J>(0 ) ) 1 + 

* = 2 



^ [&*(*) cos (fc(uy(0* + <t>(0))l 


(5.1) 


k = 2 

The parameters of the system are a, (0 , a 2 (0 (0 , b 2 (0 , . 


-,b m (t) , w f (t) , and 


<j >(*) . The signal y (t) has slow time varying frequency, amplitudes, and phases. That is: 


w f = w f (t) 


( 5.2) 


a k = a h (t) 


( 5.3) 


b k = b k (t) 


( 5.4) 


4> = 4 >(/) 


( 5.5) 
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where frequency, amplitude, and phase are nearly constant over several cycles. 



\w f (t+ 1) - w f {t)\ 

w f (t) 

|a*(f + 1) -a*( 0 | 

w r {t) 

uy(f) 

l<j>P + D -<t>(Q | , 

w f (t) 

In the representation of the signal, we did not include the (t) term cosinusoidal 
component of the fundamental frequency. This is because such a cosinusoidal frequency 
would give a phase shift in the fundamental, represented by a change in the phase of the 
fundamental. 

Regardless of the working model, we will assume that the signal is contaminated by 
additive noise. This noise v ( t ) will be white and Gaussian, which gives the measurement 
as: 



(5.6) 

(5.7) 

(5.8) 

(5.9) 



z(t) = y(t ) +u(0 ( 5.10) 

From these measurements, we will estimate aq (t) , a 2 (t), b 2 (t), 

...,6 m (0 ,w f (t) The estimation will be carried out up to the /nth harmonic. The higher 

harmonics will be assumed to be insignificant for our purposes. 

A. THE ESTIMATOR 

The extended Kalman filter will be applied for the estimation of frequency, amplitude, 
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and phase of the measurement signal. In a similar way, we will denote the state-space 
model of the rectangular form. Therefore, Equations (4.9) and (4.11) remain unchanged, 
while the others become 



x(t) = [a 1 (t)A 2 (t),... < a m (t),b 2 (t),...,b m (t),w f (t)$(t)] T 



(5.11) 



<t> = 



0 / 



m - 1 



o o 

0 0 



0 0 10 
0 0 0 1 



( 5 . 12 ) 



m 

h(x(t)) = a 1 (t)sin{(w f (t)t + $(t))) + ^ [a* (0 sin (k (w f (t) t + <> (t ) ) ) ] + 

k ~ 2 
m 

^ [&*(<) cos (k(w f (t)t + <b(t) )) ] (5.13) 

* =2 



The estimator Equations (4.18), (4.19), (4.20) and (4.21) of the polar form remain the 
same as for the rectangular form. The only difference appears in a Jacobian matrix as writ- 
ten below: 



—i 



sin( (w f (t\t- 1) +4>(<|t- 1)) 
sin (2 (Wf(t\ t- 1)< + <i>(<|* - 1) ) ) 



H(t) 



sin (m t - 1) t + 4>(t| t - 1) ) ) 

cos (2 {Wf{t\ t - 1) t + <(> (f| t - 1) ) ) 



(5.14) 



cos (m t - 1) t + <j> (*| t - 1) ) ) 

3(f|«- 1) 

P 1) 
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where, 



m 

3 (t| t - 1 ) = ka k (f| t - 1 ) tcos ( k ( U) f (t\ t - 1) t + <t> (t| t - 1 ) ) ) - 

k = 1 



m 

kb k (r| t - 1) t sin ( k ( w f (t\ t - 1) t + ^ (r| t - 1) ) ) 

k = 2 



and 



p (t\ t - 1) = ^ ka k (^| t - 1) cos ( k ( Wf(t\ t - 1) t + <j> (*l t - 1) ) ) - 

k = 1 
m 

AM*|*-l)sin(£(uy(f|r- l)f + 4>(f|f- 1))). 

k = 2 

B. SIMULATIONS 

Like the polar model, the rectangular model also has 2m+l states, with m representing 
the number of harmonics. We choose the number of harmonics m= 4 to compare with the 
polar model. The measurements consisting of sine and cosine components are 

z ( t ) = dj (0 sin ( (2jt0.04t + 0.01 ) ) +a 2 (0 sin (2 (2rc0.04t + 0.01) ) + 
a 3 (t) sin (3 (2rc0.04t + 0.01) ) +a 4 (<) sin (4 (2rc0.04t + 0.01) ) + 
b 2 (O cos (2 (2jt0.04t + 0.01) ) +b 3 (t) cos (3 (2rc0.04t + 0.01) ) + 

b 4 (t) cos (4 (2rc0.04t + 0.01) ) +t> (t) . ( 5.15) 

I 

Here v ( t ) is zero mean, white Gaussian noise with a variance of 0.1. The signal has 
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a fundamental frequency w f = 2rc0.04 and amplitudes of a k and b k for the sine and the co- 
sine components, respectively. They are defined as 



a. i 

a k = 2 k = 2,3,4 


( 5 . 16 ) 


b k = k = 2,3, 4 


( 5 . 17 ) 



The values of the amplitudes can be obtained from the desired signal to noise ratio. 
The program written in MATLAB (See Appendix C) is used for the simulations with the 
following initial conditions: 



aj = 2.4 


( 5 . 18 ) 


a 2 = 0 


( 5 . 19 ) 


a 3 = 0 


( 5 . 20 ) 


a 4 = 0 


( 5 . 21 ) 


t>2 = 1.45 


( 5 . 22 ) 


6 3 = 0 


( 5 . 23 ) 


b 4 = 0 


( 5 . 24 ) 


Wf = 2rc0.03 


( 5 . 25 ) 


O 

II 


( 5 . 26 ) 


P(0) = diag { 1, 0.5, 0.1, 0.04, 0.5, 0.1, 0.04, 0.8, 0.01} 


( 5 . 27 ) 



These initial values are chosen closely to those used in the polar form. The main dif- 
ference appears in the initial value of the error covariance matrix. The program uses the 
same value of measurement and process noise as in the polar form. 
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Figure 5.1 shows the true and estimated values of the amplitude of the first harmonic. 
Tracking is achieved after t = 50. In Figures 5.2 and 5.3, the amplitudes of the sine com- 
ponent of the second and third harmonics show a similar response to the first one. Figure 
5.4 shows the true and estimated values of the sine component of the fourth harmonic. The 
error looks worse than the others with the filter tracking after t = 150. Figures 5.5, 5.6 and 
5 7 show the estimated cosine components. Their responses are similar to the sine compo- 
nents. Figure 5.8 shows the frequency tracking of the signal. The peak value of the estima- 
tion is large compared to the one estimated in the polar form. But, like the polar form, the 
rectangular model tracks the frequency after t = 30 and locks onto it very closely. Figure 
5.9 shows the phase error of the signal. It has a steady state value of -0.0003 after t = 70. 

We also applied a case with less than four harmonics to the rectangular model. The 
measurements are assumed to be 

z(t) = Oj (0 sin (2rc0.04< + 0.01) +a 3 (f) sin (3 (2*0.04* + 0.01) ) + 

b 2 ( t ) cos (2 (2*0.04* + 0.01) ) +v (<) . ( 5.28) 

The second and the fourth harmonics of the sine component and the third and the 
fourth harmonics of the cosine component are assumed to be missing for the rectangulai 
model. Similar to the polar form, we also rearranged the error covariance matrix for the 
rectangular form as 

P(0) = diag{ 1,0.2, 0.1,0.02, 1,0.1, 0.01, 0.1, 0.001} . (5.29) 

The MATLAB program used for the simulations is shown in Appendix D. Figure 5.11 
shows the true and estimated values of the amplitude of the first harmonic. Its response i 
almost similar to the one estimated with four harmonics in the rectangular model. Figur 
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5.1 1 gives the estimation of the amplitude of the sine component of the third harmonic. Fig- 
ure 5.12 shows the amplitude tracking of the cosine component of the second harmonic, 
which is very similar to the one obtained in the four harmonics case. The true and estimated 
values of the second and the fourth amplitude harmonics for the sine component were 
around zero. The same results apply to the third and the fourth amplitude harmonics of the 
cosine component. These graphs were not included. Figures 5.13 and Figure 5.14 show the 
frequency tracking and the phase error of the signal, respectively. 



45 




Figure 5.1 True and estimate value of the amplitude. First harmonic 




Figure 5.2 True and estimate value of the amplitude. Sine component, second harmonic 
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Figure 5.3 True and estimate value of the amplitude. Sine component, third harmonic 




Figure 5.4 True and estimate value of the amplitude. Sine component, fourth harmonic 
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Figure 5.5 True and estimate value of the amplitude: Cosine component, second harmoni 
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Figure 5.6 True and estimate value of the amplitude: Cosine component, third harmonic 




Figure 5.7 True and estimate value of the amplitude. Cosine component, fourth harmonic 
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Figure 5.8 True and estimate value of the fundamental frequency 




Figure 5.9 Phase error of the signal 
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Figure 5.10 Amplitude tracking of the first harmonic. (Three harmonic case) 




Figure 5.11 Amplitude tracking. Sine component, third harmonic. (Three harmonic case) 
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Figure 5.12 Amplitude tracking. Cosine component, second harmonic. 

(Three harmonic case) 
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Figure 5.13 Frequency tracking. (Three harmonic case) 




Figure 5.14 Phase error. (Three harmonic case) 
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VI. CONCLUSIONS 



This thesis presents the estimation of frequency, amplitude, and phase of a signal using 
an extended Kalman filter algorithm. Both the polar and the rectangular model filter were 
implemented. In both models, the signal is contaminated by noise. 

The simulations show that the performance of both filters are similar. Although there 
is little difference between the two models, the rectangular model seems to be able to 
estimate phase better than the polar model. For both filters, the tracking of the fundamental 
frequency plays a significant role. If the estimated value of the frequency locks onto a 
multiple of the true frequency, the filter cannot track the amplitudes. Each model offers a 
different advantage. The estimation of amplitudes of the signal is more convenient to 
measure in the polar model than in the rectangular model. 

In all the simulations, an 18 dB signal-to-noise ratio was used. The performance of the 
two filters was poor when lower signal-to-noise ratios were used. The signals may have a 
Doppler shift. By adding another state, additional Doppler effects can be accounted for in 
future work. 



\ 
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APPENDIX A 



%This program tracks the frequency, amplitude, and the 
phase of the nonperiodic signal in noise by using extended 
Kalman filter algorithm for the polar model. 

% State space model 



F= [ 1 0 0 0 
0 10 0 
0 0 10 
0 0 0 1 
0 0 0 0 
0 0 0 0 
0 0 0 0 
0 0 0 0 
0 0 0 0 



0 0 0 0 0 

0 0 0 0 0 

0 0 0 0 0 

0 0 0 0 0 

1 0 0 0 0 

0 10 0 0 
0 0 10 0 
0 0 0 1 0 

00001 ]; 



% Initial conditions of the true state 
rl(l)=2.8; %Amplitude of the first component 
r2 ( 1 ) =rl ( 1 ) /2 ; %Amplitude of the second component 
r3 (1) =r2 (1) / 2 ; %Amplitude of the third component 
r4 ( 1) =r3 ( 1) /2 ; %Amplitude of the fourth component 
01(1) =0.01; %Phase of the first component 
02(1) =0.01; %Phase of the second component 
03(1) =0.01; %Phase of the third component 
04(1) =0.01; %Phase of the fourth component 
w (1) =2*pi*0 . 04; %Fundametal frequency 



%Initial condition of the estimation 
xkkm ( : , 1 ) = [ 2 . 6 ; 0 ; 0 ; 0 ; 2 *pi * 0 . 0 3 ; 0 ; 0 ; 0 ; 0 ] ; 



R=0.1; % measurement noise 
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ql=le-3; % process noise 
q2=le-7 ; 
q3=le-7 ; 



Q= tqi 


0 


0 


0 


0 0 0 0 0 


0 


qi 


0 


0 


0 0 0 0 0 


0 


0 


qi 


0 


0 0 0 0 0 


0 


0 


0 


qi 


0 0 0 0 0 


0 


0 


0 


0 


q2 0 0 0 0 


0 


0 


0 


0 


0 q3 0 0 0 


0 


0 


0 


0 


0 0 q3 0 0 


0 


0 


0 


0 


0 0 0 q3 0 


0 


0 


0 


0 


0 0 0 0 q3 ] ; 


%Error 


covariance matrix 


P=[6 


0 


0 


0 


0 0 0 0 0 


0 


5 


0 


0 


0 0 0 0 0 


0 


0 


3 


0 


0 0 0 0 0 


0 


0 


0 


2 


0 0 0 0 0 


0 


0 


0 


0 


0.08 0000 


0 


0 


0 


0 


0 0.001 000 


0 


0 


0 


0 


0 0 0.001 0 0 


0 


0 


0 


0 


000 0.001 0 


0 


0 


0 


0 


0000 0.001]; 



%Jacobian matrix 
H ( 1 , : ) = [ 0 0 0 0 0 0 0 0 0 ] ; 

%Transpose of the Jacobian matrix 
Ht ( : , 1 ) =H ( 1 , : ) ' ; 



t=l:450; %Number of samples 

%True state matrix 
x ( : , 1) = [rl (1 ) 
r2 (1) 
r3 ( 1 ) 
r4 (1) 
w (1) 

01 ( 1 ) 

02 ( 1 ) 

03 (1) 
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04 (1) ] ; 



for i = 1:450 
rand ( 'normal ' ) 

%Observation 

z(i)=x(l,i)*sin(x(5,i) *i + x(6, i) ) + 
x(2,i) *sin(2*x(5,i) * i+x (7 , i ) )+x(3,i) *sin(3*x(5,i) * i + x ( 8 , i) ) + 
x ( 4 , i ) *sin(4*x(5,i) * i + x ( 9, i ) )+0. 1 * rand ( 1 ) / 



he ( i ) =xkkm ( 1 , i ) *sin (xkkm (5, i ) * i+xkkm ( 6, i ) ) +xkkm (2, i) *sin (2 *x 
kkm (5, i) * i+xkkm (7 , i) ) +xkkm(3, i) *sin (xkkm (5,i) *3*i+xkkm(8,i) ) 
+ xkkm (4, i) * sin (xkkm (5, i) *4 * i+xkkm ( 9, i) ) ; 

% Kalman gain 

L ( : , i) =P*H (i, : ) ' *inv (H (i, : ) *P*H (i, : ) ' +R) ; 

% Estimate update 

xkk ( : , i) =xkkm ( : , i) +L ( : , i) * (z (i) - he(i)); 

xkkm ( : , i + 1 ) =F*xkk ( : , i) ; 

%Tranpose of the Jacobian matrix 

Ht ( : , i + 1) = [ sin (xkkm (5, i) * i + xkkm ( 6, i) ) ; 

sin (2*xkkm (5, i) *i+xkkm(7, i) ) ; 

sin (xkkm (5, i) *3* i + xkkm (8, i) ) ; 

sin (xkkm (5, i) *4* i+xkkm (9, i) ) ; 

xkkm (1, i) * i *cos (xkkm (5, i) * i + xkkm ( 6, i) ) +xkkm(2, i) *2*i*cos (2 *x 
kkm (5, i) * i+xkkm (7, i) ) . . 

+ xkkm (3, i) *3*i*cos (3*xkkm(5, i) * i+xkkm (8, i) ) +xkkm (4 , i) *4*i*co 
s (4*xkkm (5, i) * i + xkkm (9, i) ) ; 
xkkm ( 1 , i ) *cos (xkkm (5, i) *i+xkkm(6, i) ) ; 
xkkm (2, i) *cos (2*xkkm (5, i) * i+xkkm (7, i) ) ; 
xkkm (3, i) *cos (3* xkkm (5, i) *i + xkkm(8, i) ) ; 
xkkm (4 , i) *cos (4 *xkkm (5, i) * i+xkkm ( 9, i) ) ) ; 

% Jacobian 

H ( i + 1 , : ) =Ht ( : , i + 1) ' / 

% Error covariance update 
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P=F*(P-L(:,i)*H(i, : ) *P ) *F ' + Q; 



v ( : , i) = [rand (1) *le-2 

rand ( 1 ) * le-2 

rand ( 1 ) * le-2 

rand ( 1 ) * le-2 

rand ( 1 ) * le-4 

rand ( 1 ) * le-4 

rand (1) *le-4 

rand (1) *le-4 

rand ( 1 ) * le-4 ] ; 

%State update 

x ( : , i+1) =F*x ( : , i) +v(:,i); 
end 

plot (t , x ( 1 , 1 : i) ,t,xkk(l,l:i) ) , grid, xlabel ( 'Time ' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tezlal 

plot (t, x (2, 1 : i) , t , xkk (2, 1 : i) ) ; pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tezla2 

plot(t,x(3,l:i),t, xkk (3, 1 : i) ) /pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ) , meta tezla3 

plot (t,x (4, 1 : i ) ,t,xkk (4, 1 : i) ) /pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State'), meta tezla4 

plot(t,x(5,l:i) ,t, xkk (5, 1 : i) ) /pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Frequency' ) , gtext ( '-True State' ) , gtext ( ' — Estimate 
of the State'), meta tezla5 

plot (t, x (6, 1 : i) -xkk (6, 1 : i) ) /pause, grid, 

xlabel ( 'Time' ), ylabel ( 'Phase Error'), meta tezla6 

plot(t,x(7,l:i) -xkk (7, 1 : i) ) / pause, grid, 

xlabel ( 'Time' ), ylabel ( 'Phase Eror'),meta tezla7 
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plot (t , x (8, 1 : i) -xkk (8, 1 : i) ) ; pause, grid, 

xlabel ( 'Time' ), ylabel ( 'Phase Error' ),meta tezla8 

plot (t , x ( 9, 1 : i) -xkk (9, 1 : i) ) ; pause, grid, 

xlabel ( 'Time' ), ylabel ( 'Phase Error' ),meta tezla9 
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APPENDIX B 



%This program computes the frequency, amplitude and the 
phase of the, nonperiodic signal in noise which consists of 
two harmonics for the polar model.lt uses extended Kalman 
filter algorithm. 



% State space model 



F=[l 0 0 0 0 0 
0 1 0 0 0 0 
0 0 1 0 0 0 
0 0 0 1 0 0 
0 0 0 0 1 0 
0 0 0 0 0 1 
0 0 0 0 0 0 
0 0 0 0 0 0 
0 0 0 0 0 0 



0 0 0 
0 0 0 
0 0 0 
0 0 0 
0 0 0 
0 0 0 
10 0 
0 10 
0 0 1 ]; 



% Initial conditions of the true state 

rl(l)=2.8; %Amplitude of the first component 

r2 ( 1 ) =0 ; %rl ( 1 ) /2 ; %Amplitude of the second component 

r3 ( 1 ) =rl ( 1 ) /4 ; %Amplitude of the third component 

r4 (1) =0; %r3 (1) /2; %Amplitude of the fourth component 

01(1) =0.01/ %Phase of the first component 

02 (1 ) =0; %0 . 01; %Phase of the second component 

03(1) =0.01; %Phase of the third component 

04 ( 1 ) =0 ; %0 . 01 ; %Phase of the fourth component 

w ( 1 ) =2 *pi * 0 . 04 ; %Fundametal frequency 

%Initial condition of the estimation 
xkkm ( : , 1 ) = [ 2 . 6 ; 0 ; 0 ; 0 ; 2 *pi * 0 . 02 ; 0 ; 0 ; 0 ; 0 ] ; 

R=0.1; % measurement noise 
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process noise 



ql=le-3; % 

q2=le-7; 

q3=le-7; 



Q= [qi 


0 


0 


0 


0 0 0 0 0 




0 


qi 


0 


0 


0 0 0 0 0 




0 


0 


qi 


0 


0 0 0 0 0 




0 


0 


0 


qi 


0 0 0 0 0 




0 


0 


0 


0 


q2 0 0 0 0 




0 


0 


0 


0 


0 q3 0 0 0 




0 


0 


0 


0 


0 0 q3 0 0 




0 


0 


0 


0 


0 0 0 q3 0 




0 


0 


0 


0 


0 0 0 0 q3 ] ; 




%Error 


covariance matrix 


P= [2 


0 


0 


0 


0 0 0 0 0 




0 


1 


0 


0 


0 0 0 0 0 




0 


0 


0. 


5 


0 0 0 0 0 0 




0 


0 


0 


0. 


2 0 0 0 0 0 




0 


0 


0 


0 


0.04 0000 




0 


0 


0 


0 


0 0.001 0 0 


0 


0 


0 


0 


0 


0 0 0.001 0 


0 


0 


0 


0 


0 


000 0.001 


0 


0 


0 


0 


0 


0000 0.001] ; 



%Jacobian matrix 
H ( 1 , : ) = [ 0 0 0 0 0 0 0 0 0 ] ; 

%Transpose of the Jacobian matrix 
Ht < : , 1 ) =H ( 1 , : ) ' ; 



t=l : 450; 

%True state matrix 
x ( : , 1) = [rl (1) 
r2 (1) 
r3 (1) 
r4 (1) 
w (1) 

01 ( 1 ) 

02 ( 1 ) 
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03 (1) 

04 (1) ] ; 



for i = 1:450 
rand ( 'normal ' ) 

%0bservation 

z (i)=x(l,i) *sin (x (5, i) *i + x(6,i) ) + 
x(2,i)*sin(2*x(5,i)*i + x(7,i))+x(3,i)*sin(3*x(5,i)*i+x(8,i)) + 
x (4 , i ) *sin(4*x(5,i) *i+x(9,i) )+0. 1 * rand (1 ) ; 



he ( i ) =xkkm (1, i) * sin (xkkm(5, i) * i+xkkm (6, i) ) +xkkm (2, i) *sin (2 *x 
kkm (5, i) * i+xkkm (7, i) ) +xkkm (3, i) *sin (xkkm (5, i) *3*i+xkkm (8, i) ) 
+xkkm ( 4 , i) *sin (xkkm (5, i) *4 * i+xkkm ( 9, i) ) ; 

% Kalman gain 

L ( : , i) =P*H (i, : ) ' *inv(H (i, : ) *P*H (i, : ) ' +R) ; 

% Estimate update 

xkk ( : , i) =xkkm ( : , i) +L ( : , i) * (z (i) - he(i)); 
xkkm ( : , i + 1 ) =F*xkk ( : , i ) ; 

%Tranpose of the Jacobian matrix 

Ht ( : , i + 1 ) = [ sin (xkkm (5 , i ) * i+xkkm ( 6, i ) ) ; 

sin(2*xkkm(5,i) * i+xkkm (7, i) ) ; 

sin (xkkm (5 , i ) *3* i+xkkm (8, i) ) ; 

sin (xkkm (5, i) *4*i+xkkm (9, i) ) ; 

xkkm(l, i) *i*cos (xkkm (5, i) *i+xkkm(6, i) ) +xkkm(2, i) *2*i*cos (2*x 
kkm (5, i) * i+xkkm (7, i) ) 

+xkkm (3, i) *3*i*cos (3* xkkm (5, i) * i+xkkm ( 8 , i) ) +xkkm (4, i) *4*i*co 
s (4*xkkm (5, i) * i+xkkm (9, i) ) ; 
xkkm (1, i) *cos (xkkm (5, i) *i+xkkm(6, i) ) ; 
xkkm (2, i) *cos (2*xkkm (5, i) * i+xkkm (7, i) ) / 
xkkm (3, i) *cos (3*xkkm (5, i) * i+xkkm (8, i) ) ; 
xkkm (4 , i ) *cos (4 *xkkm (5, i ) * i+xkkm ( 9, i ) ) ] ; 

% Jacobian 

H (i+1, : ) =Ht ( : , i+1 ) ' ; 
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% Error covariance update 
P=F* (P-L(:,i)*H(i, :)*P)*F'+ Q; 

v ( : , i) = [ rand (1 ) * le-2 

rand ( 1 ) * le-2 

rand ( 1 ) * le-2 

rand ( 1 ) * le-2 

rand ( 1 ) *le-4 

rand ( 1 ) * le-4 

rand ( 1 ) * le-4 

rand ( 1 ) * le-4 

rand (1) *le-4 ] ; 

%State update 

x ( : , i + 1 ) =F*x ( : , i ) + v(:,i); 

end 

plot(t,x(l, 1 : i) , t , xkk ( 1 , 1 : i ) ) ; pause, grid,xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ) , meta tezlbl 

plot (t, x (2, 1 : i) ,t,xkk(2,l:i) ) /pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State'), meta tezlb2 

plot (t, x (3, 1 : i) , t, xkk (3, 1 : i) ) /pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State'), meta tezlb3 

plot (t , x (4 , 1 : i) , t , xkk (4 , 1 : i) ) /pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State'), meta tezlb4 

plot (t , x (5, 1 : i ) , t , xkk (5, 1 : i) ) /pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State'), meta tezlb5 

plot(t,x(6,l:i) -xkk (6, 1 : i) ) / pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State'), meta tezlb6 

plot (t, x (7, 1 : i) -xkk (7, 1 : i) ) /pause, grid, xlabel ( 'Time' ) , 
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ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tezlb7 

plot (t , x (8, l:i) -xkk (8, 1 : i) ) /pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tezlb8 

plot(t,x(9,l:i) -xkk (9, 1 : i) ) / pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tezlb9 
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APPENDIX C 



%This program trackes the frequency, amplitude, and the phase of the nonperiodic 
signal in noise by using extended Kalman filter algorithm for the rectangular model. 



% State space model 



F= [ 1 0 0 0 
0 10 0 
0 0 10 
0 0 0 1 
0 0 0 0 
0 0 0 0 
0 0 0 0 
0 0 0 0 
0 0 0 0 



0 0 0 0 0 

0 0 0 0 0 

0 0 0 0 0 

0 0 0 0 0 

1 0 0 0 0 

0 10 0 0 
0 0 10 0 
0 0 0 1 0 

00001 ]; 



% Initial conditions of the true state 
a ( 1 ) = 2 . 7 ; %Amplitude of the first component. 
a2 (1) =a (1) /2; %Amplitude of the second component 
a3 ( 1 ) =a (2 ) /2 ; %Amplitude of the third component. 
a4 ( 1 ) =a ( 3 ) /2 ; %Ampl itude of the fourth component 
w(l)=2*pi*0.04; %Fundamental frequency . 

01 ( 1 ) =0 . 01 ; %Phase of the first component 
b2 (1) =a (1) /2; %Phase of the second component 
b3 (1) =a (2) /2;%Phase of the third component 
b4 (1) =a (2) /2;%Phase of the fourth component 



%Initial condition of the estimation 
xkkm ( : , 1 ) = [ 2 . 4 ; 0 ; 0 ; 0 ; 1 . 4 50 ; 0 ; 0 ; 2 *pi * 0 . 0 3 ; 0 ] ; 



R=0.1; % Measurement noise 



ql=le-3; % process noise 

q2=le-7; 

q3=le-7 ; 
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Q=[ql 00000000 
Oql 0000000 
00 ql 000000 
000 ql 00000 
0000 ql 0000 
00000 ql 000 
000000 ql 00 
0000000 q20 
00000000 q3 ] ; 



%Error covariance matrix. 

Pkk= [ 1 00000000 

00. 5 0000000 
00 0.1 000000 
000 0.04 00000 
0000 0.5 0000 
00000 0.1 000 
000000 0.04 0 0 
00000000. 80 
0 0 0 0 0 0 0 0 0 . 001 ]; 



%Jacobian matrix. 

H ( 1 , : ) = [ 0 0 0 0 0 0 0 0 0 ] ; 

%Transpose of the Jacobian matrix. 
Ht ( : , 1 ) = [ 0 0 0 0 0 0 0 0 0 ] ' ; 



t=l : 450 ; %Number of samples 

%True state matrix 
x ( : , 1) = [a (1) 
a2 (1) 
a3 ( 1 ) 
a 4 (1) 
b2 (1) 
b3 (1) 
b4 (1) 
w (1) 

01 ( 1 ) ] ; 



66 



for i= 1:450 
rand ( 'normal ' ) 



z(i)=x(l,i)*sin(x(8,i)*i+x(9,i) )+x(2,i)*sin(2* (x(8,i)*i + x(9, 
i) ) )+x(3,i)*sin(3*(x(8,i) *i+x ( 9, i ) ) ) +x ( 4 , i ) * sin ( 4 * ( i *x ( 8 , i ) + 
x (9, i) ) ) +x (5, i) *cos (2* (x (8, i) *i+x (9, i) ) )+x(6,i) *cos (3* (x (8, i 
)*i+x(9,i)))+x(7,i)*cos(4*(x(8,i)*i+x(9,i)))+0.1*rand(l); 

%Variable 

je ( i ) =xkkm ( 1 , i ) *sin(xkkm(8,i) * i+xkkm (9,i) ) +xkkm(2,i) * sin ( 2 * ( 
xkkm ( 8 , i ) * i + xkkm (9,i) ) ) +xkkm ( 3 , i ) *sin(3* (xkkm ( 8 , i ) * i+xkkm ( 9, 
i ) ) ) +xkkm ( 4 , i ) * sin ( 4 * ( i *xkkm ( 8 , i ) +xkkm ( 9, i) ) ) ; 

%Variable 

ke ( i ) =xkkm ( 5, i ) *cos ( 2 * (xkkm ( 8 , i ) * i+xkkm ( 9, i ) ) ) +xkkm (6, i ) * cos 
( 3 * (xkkm ( 8 , i ) * i + xkkm ( 9, i ) ) ) +xkkm (7 , i ) *cos ( 4 * (xkkm (8, i) * i+xkk 
m ( 9 , i ) ) ) ; 

he (i) = je (i) +ke (i) ; 

%Time update of the error covariance 
Pkkm=F*Pkk *F' +Q; 

% Kalman gain 

K ( : , i) =Pkkm*H (i, : ) ' *inv (H (i, : ) *Pkkm*H (i, : ) ' +R) ; 

% Estimate update 

xkk ( : , i) =xkkm ( : , i) +K ( : , i) * (z (i) - he(i)); 

xkkm ( : , i+1 ) =F*xkk ( : , i ) ; 

%Variable 

c (i) =xkkm (1, i) *i*cos (xkkm (8, i) * i + xkkm (9, i) ) +xkkm (2, i) *2*i*co 
s (2* (xkkm (8, i) *i+xkkm(9, i) ) ) + xkkm (3, i) *3*i*cos (3* (xkkm (8, i) * 
i+xkkm ( 9, i ) ) ) +xkkm (4,i)*4*i*cos(4* (i*xkkm (8, i) +xkkm (9, i) ) ) ; 

%Variable 

d (i) =-xkkm (5,i) *2*i*sin(2* (xkkm (8, i) * i+xkkm (9, i) ) ) - 
xkkm (6, i) *3*i*sin (3* (xkkm (8,i) *i+xkkm(9,i) ) ) - 
xkkm (7,i) *4*i*sin(4* (xkkm (8, i) * i+xkkm (9, i) ) ) ; 



67 



%Variable 

f (i) =xkkm (1, i) *cos (xkkm (8, i) *i+xkkm (9, i) ) +xkkm (2,i) *2* cos (2* 
(xkkm (8, i) * i+xkkm (9, i) ) ) +xkkm (3,i)*3*cos(3* (xkkm (8, i) * i+xkkm 
( 9, i ) ) ) +xkkm (4 , i) *4* cos (4* (i* xkkm (8, i) +xkkm ( 9, i ) ) ) ; 
g (i) =-xkkm (5, i) *2*sin (2* (xkkm (8, i) * i+xkkm (9,i)))- 
xkkm (6, i) *3*sin(3* (xkkm (8, i) * i+xkkm (9, i) ) ) - 
xkkm (7, i) *4*sin (4* (xkkm (8, i) *i + xkkm(9, i) ) ) ; 

%transpose of the Jacobian 
Ht(:,i + l) = (sin(xkkm(8 / i) *i+xkkm (9, i) ) ; 
sin (2* (xkkm ( 8 , i) * i+xkkm (9, i) ) ) / 
sin (3* (xkkm (8, i) * i+xkkm (9, i) ) ) ; 
sin (4 * (xkkm ( 8 , i) * i+xkkm (9, i ) ) ) ; 
cos (2* (xkkm ( 8 , i) * i+xkkm ( 9, i) ) ) / 
cos (3* (xkkm (8, i) * i + xkkm (9, i) ) ) ; 
cos (4 * (xkkm ( 8 , i ) * i + xkkm ( 9, i ) ) ) ; 
c (i) +d (i) ; 
f (i) +g (i) ] ; 

% Jacobian 

H ( i+1 , : ) =Ht ( : , i+1 ) ' ; 

%Measurement update of error covariance 
Pkk=(eye(9) - K ( : , i) *H (i, : ) ) *Pkkm; 

v(:,i) = [rand (1) *le-2 
rand ( 1 ) * le-2 
rand ( 1 ) * le-2 
rand (1) *le-2 
rand ( 1 ) *le-2 
rand (1 ) *le-2 
rand (1) *le-2 
rand (1) *le-4 
rand (1) *le-4] ; 

%State update 

x ( : , i + 1 ) =F*x ( : , i) + v(:,i); 
end 

plot (t, x (1, 1 : i) , t, xkk (1 , 1 : i) ) , grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tez52cl 
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plot(t,x(2,l:i),t,xkk(2,l:i)) / pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tez52c2 

plot (t,x(3, 1 : i) ,t,xkk (3, 1 : i) ) /pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tez52c3 

plot (t , x (4 , 1 : i) , t , xkk (4 , 1 : i ) ) /pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tez52c4 

plot (t , x (5, 1 : i) , t, xkk (5, 1 : i) ) /pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tez52c5 

plot(t,x(6,l:i) ,t,xkk(6,l:i) ) / pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ) , meta tez52c6 

plot(t,x(7,l:i),t,xkk(7,l:i)) / pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ), gtext ( '-True State' ) , gtext ( '--Estimate 
of the State'), meta tez52c7 

plot (t, x (8, 1 : i) -xkk (8, 1 : i) ) /pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Phase Eror' ), gtext ( '--Estimate of the State'), meta 
tez52c8 

plot(t,x(9,l:i),t, xkk (9, 1 : i) ) / pause, grid, 

xlabel ( 'Time' ), ylabel ( 'Phase Eror'), meta tezla9 
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APPENDIX D 



%This program trackes the frequency, amplitude, and the phase of the nonperiodic 
signal in noise which consists of two harmonics. It uses extended Kalman filter algorithm 
for the rectangular model. 

% State space model 

F= [100000000 
010000000 
001000000 
000100000 
000010000 
000001000 
000000100 
000000010 
000000001 ]; 

% Initial conditions of the true state 
a ( 1 ) =2 . 7 ; %Amplitude of the first component. 
a2 (1) =0; %Amplitude of the second component. 
a3 ( 1 ) =a (2) /2 ; %Amplitude of the third component. 
a4 (1) =0; %Amplitude of the fourth component. 
w(l)=2*pi*0.04; %Fundamental frequency . 

01 ( 1 ) =0 . 0 1 ; %Phase of the first component 
b2 ( 1) =a ( 1 ) /2 ; %Phase of the second component 
b3 ( 1 ) =0 ; %Phase of the third component 
b4 ( 1 ) =0 ; %Phase of the fourth component 

%Initial condition of the estimation 
xkkm (:,l)=[2.4;0;0;0;1.450;0;0;2*pi*0.03;0]; 

R=0.1; % Measurement noise 

ql=le-3; % process noise 
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q2=le-7 ; 
q3=le-7; 

Q= [ q 1 00000000 
Oql 0000000 
00 ql 000000 
000 ql 00000 
0000 ql 0000 
00000 ql 000 
000000 ql 00 
0000000 q20 
00000000 q3 ] ; 



%Error covariance matrix. 

Pkk= [ 1 00000000 

00. 2 0000000 
00 0.1 000000 
000 0.02 00000 
000010000 
00000 0.1 000 
000000 0.01 0 0 
0000000 0. 1 0 
00000000 0 . 001 ]; 



%Jacobian matrix. 

H ( 1 , : ) = [ 0 0 0 0 0 0 0 0 0 ] ; 

%Transpose of the Jacobian matrix. 
Ht ( : / 1 ) = [ 0 0 0 0 0 0 0 0 0 ] ' ; 

t=l : 4 50 ; %Number of samples 

%True state matrix 
x(:,l) = [a(l) 
a2 (1) 
a3 (1) 
a4 (1) 
b2 (1) 
b3 (1) 
b4 (1) 
w (1) 

01 ( 1 ) ] ; 
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for i= 1:450 
rand ( 'normal ' ) 



z (i) =x (1, i) *sin (x (8, i) *i+x (9, i) ) +x (2, i) *sin (2* (x(8,i) *i + x(9, 
i) ) )+x(3,i)*sin(3* (x(8,i)*i+x(9,i) ) )+x(4,i) *sin(4* (i*x (8, i) + 
x (9, i) ) )+x(5,i) *cos (2* (x(8,i)*i+x(9,i) ) ) +x (6, i) *cos (3* (x (8, i 
) * i + x (9, i) ) ) +x (7, i) *cos (4* (x (8, i) * i+x (9, i) ) ) +0 . 1 * rand ( 1 ) ; 

%Variable 

je (i ) =xkkm ( 1 , i ) *sin (xkkm (8 , i ) * i+xkkm (9, i ) ) +xkkm (2, i) *sin(2* ( 
xkkm(8, i) *i+xkkm ( 9, i) ) ) +xkkm (3 , i) *sin (3* (xkkm ( 8 , i) * i+xkkm ( 9, 
i) ) ) +xkkm (4, i) *sin (4* (i*xkkm (8, i) +xkkm (9, i) ) ) ; 

%Variable 

ke (i) =xkkm (5, i) *cos (2* (xkkm (8, i) * i+xkkm (9, i) ) ) +xkkm (6, i) *cos 
(3* (xkkm (8, i) * i+xkkm (9, i) ) ) +xkkm (7, i) *cos (4* (xkkm (8, i) *i+xkk 
m(9, i) ) ) ; 

he (i) = je (i) +ke (i) / 

%Time update of the error covariance 
Pkkm=F*Pkk*F' +Q; 

% Kalman gain 

K ( : , i ) =Pkkm*H ( i , : ) ' *inv (H (i, : ) *Pkkm*H (i, : ) ' +R) ; 

% Estimate update 

xkk ( : , i) =xkkm ( : , i) +K ( : , i) * (z (i) - he(i)); 
xkkm ( : , i+1) =F*xkk ( : , i) ; 

%Variable 

c (i) =xkkm(l, i) *i*cos (xkkm (8, i) * i+xkkm ( 9, i ) ) +xkkm (2, i) *2*i*co 
s (2* (xkkm (8, i) * i+xkkm (9, i) ) ) +xkkm(3, i) *3*i*cos (3* (xkkm (8, i) * 
i+xkkm (9, i) ) ) +xkkm(4, i) *4*i*cos (4* ( i*xkkm ( 8 , i ) +xkkm ( 9, i ) ) ) ; 

%Variable 
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d (i ) =-xkkm (5, i) *2*i*sin(2* (xkkm ( 8 , i ) * i+xkkm ( 9, i ) ) ) - 
xkkm (6, i) *3*i*sin (3* (xkkm ( 8 , i ) * i+xkkm ( 9, i ) ) ) - 
xkkm (7,i)*4*i*sin(4* (xkkm ( 8 , i ) * i + xkkm ( 9, i ) ) ) ; 

%Variable 

f (i ) =xkkm ( 1 , i ) *cos (xkkm ( 8 , i ) * i + xkkm ( 9, i ) ) +xkkm (2, i) * 2 * cos (2* 
(xkkm ( 8 , i ) * i+xkkm ( 9 , i ) ) ) +xkkm (3, i) *3* cos (3* (xkkm ( 8 , i ) * i+xkkm 
(9,i) ) )+xkkm(4,i) *4*cos(4* ( i *xkkm ( 8 , i ) +xkkm ( 9, i) ) ) ; 
g (i) =-xkkm (5, i) *2*sin (2* (xkkm (8, i) *i+xkkm(9,i) ) ) - 
xkkm (6,i) *3*sin(3* (xkkm (8, i) * i + xkkm (9, i) ) ) - 
xkkm (7, i) *4*sin (4* (xkkm (8 , i ) * i+xkkm ( 9, i ) ) ) ; 

%transpose of the Jacobian 
Ht(:,i + l) = [sin (xkkm ( 8 , i ) * i + xkkm ( 9, i) ) ; 
sin (2* (xkkm (8, i) *i+xkkm(9,i) ) ) ; 
sin (3* (xkkm (8, i) *i+xkkm(9, i) ) ) ; 
sin (4* (xkkm (8, i) * i + xkkm ( 9, i ) ) ) ; 
cos (2* (xkkm (8 , i) * i + xkkm (9, i) ) ) ; 
cos (3* (xkkm (8, i) *i + xkkm(9, i) ) ) ; 
cos (4 * (xkkm ( 8 , i ) * i + xkkm ( 9, i ) ) ) ; 
c (i) +d (i) ; 
f (i) +g (i) ] ; 

% Jacobian 

H (i + 1, : ) =Ht ( : , i + 1) ' ; 

%Measurement update of error covariance 
Pkk= (eye ( 9) - K ( : , i) *H (i, : ) ) *Pkkm; 

v(:,i) = [ rand ( 1 ) * le-2 
rand ( 1 ) * le-2 
rand ( 1 ) * le-2 
rand ( 1 ) * le-2 
rand ( 1 ) * le-2 
rand ( 1 ) *le-2 
rand ( 1 ) * le-2 
rand ( 1 ) * le-4 
rand ( 1 ) * le-4 ] ; 

%State update 

x(:,i + l)=F*x(:,i) + v ( : , i ) ; 
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end 

plot(t,x(l,l:i),t, xkk (1, 1 : i) ) ,grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( ' — Estimate 
of the State' ),meta tez52cl 

plot(t,x(2,l:i),t, xkk (2, 1 : i) ) ; pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tez52c2 

plot (t, x (3, 1 : i) ,t,xkk(3, 1 : i) ) ; pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tez52c3 

plot (t,x (4 , 1 : i) ,t,xkk (4, 1 : i) ) ; pause , grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( ' — Estimate 
of the State' ),meta tez52c4 

plot (t, x (5, 1 : i) ,t,xkk(5,l:i) ) / pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( ' — Estimate 
of the State' ),meta tez52c5 

plot (t, x (6, 1 : i) , t, xkk (6, 1 : i) ) /pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tez52c6 

plot (t , x (7, 1 : i) , t , xkk (7, 1 : i) ) /pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True State' ) , gtext ( '--Estimate 
of the State' ),meta tez52c7 

plot (t, x (8, 1 : i) -xkk (8, 1 : i ) ) /pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Frequency '), gtext ( '--Estimate of the State' ),meta 
tez52c8 

plot (t, x (9, 1 : i) ,t, xkk (9, 1 : i) ) /pause, grid, 

xlabel ( 'Time' ), ylabel ( 'Phase Eror'),meta tez52c9 
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